package org.scalpel.common;

/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * A keypoint is a 2D point used to define a trajectory.
 * It is use to define robot target or position 
 **/
public class KeyPoint {
  
  public double x = 0.f;
  public double y = 0.f;
  public double a = 0.f;
  public double d = 0.f;
  
  
  public KeyPoint( ){
  	super();
  	x = 0.;
    y = 0.;
    a = 0.;
    d = 0.;
  }
  
  
  public KeyPoint( KeyPoint k ){
  	super();
    this.x = k.x;
    this.y = k.y;
    this.a = k.a;
    this.d = k.d;
  }
  
  
  public KeyPoint( double [] f ){
  	super();
    this.x = f[0];
    this.y = f[1];
    this.a = 0.f;
    this.d = 0.f;
    if( f.length > 2 ) this.a = f[2];
    if( f.length > 3 ) this.d = f[3];
  }
  
  
  public KeyPoint(double x, double y) {
  	super();
    this.x = x;
    this.y = y;
    this.a = 0.f;
    this.d = 0.f;
	}
  
  
  public KeyPoint(double x, double y, double a, double d) {
  	super();
    this.x = x;
    this.y = y;
    this.a = a;
    this.d = d;
	}
  
  public void rotate(KeyPoint center, double angle){
		double xtmp = Math.cos(angle)*(x - center.x) + Math.sin(angle)*(y - center.y) + center.x;
	double ytmp = -Math.sin(angle)*(x - center.x) + Math.cos(angle)*(y - center.y) + center.y;
		
		x = xtmp;
		y = ytmp;
		a += angle;
	}
	@Override
  protected KeyPoint clone() throws CloneNotSupportedException {
    return new KeyPoint(this);
  }

  @Override
  public String toString() {
    return Double.toString(this.x) + ", " + Double.toString(this.y) + ", " + Double.toString(this.a);
  }
  
  public KeyPoint add(KeyPoint o){
  	this.x += o.x;
  	this.y += o.y;
  	this.a += o.a;
  	this.d += o.d;
		return this;
  }
  
}
